%
%
% 运行时间4-6min!!!!请耐心等待
%
%
%
% 打开文件
fIMU          = fopen('IMU.bin', 'r');%原始IMU数据
fPureIns      = fopen('PureINS.bin','r');%参考结算结果
fcontrast0    = fopen('contrast0.txt','w');%示例数据的差值
fcar          = fopen('小推车惯导数据.ASC','r');%打开自己小推车采集的数据
fresult0      = fopen('result0.txt','w');%示例数据计算结果
fresult1      = fopen('result1.txt','w');%不进行零速修正计算结果
fresult2      = fopen('result2.txt','w');%零速修正计算结果
fcontrast1    = fopen('contrast1.txt','w');%不进行零速修正结果和参考结果的差值
fcontrast2    = fopen('contrast2.txt','w');%进行零速修正结果和参考结果的差值
f4Nav         = fopen('4.Nav','r');%打开小推车参考数据
fNE1          = fopen('NE1.txt','w');%打开无零速修正的轨迹文件
fNE2          = fopen('NE2.txt','w');%打开有零速修正的轨迹文件
fNETrue       = fopen('NETrue.txt','w');%参考轨迹文件
%%
%初始化条件(参数：our代表我们的小推车数据，example代表示例数据)
time_k_1      = Initialization("time_k_1","example");
cbn_k_1       = Initialization("cbn_k_1","example");
position_k_2  = Initialization("position_k_2","example");
position_k_1  = Initialization("position_k_1","example");
v_k_2n        = Initialization("v_k_2","example");
v_k_1n        = Initialization("v_k_1","example");
%获取更新需要的"上一个历元数据"(参数：fIMU、example代表参考数据，fcar、our代表小推车数据)
data_k_1      = TimeAalignment(fIMU,7,time_k_1,"example");
% 循环读取数据，直到到达文件末尾
while true

    %这里的文件指针保留TimeAalignment函数使用过的文件指针，从time_k_1后一时刻开始读入
    %"(参数：fIMU、example代表参考数据，fcar、our代表小推车数据)
    data_k = DataRead(fIMU, 7, 'double', "example");
    if isempty(data_k)
        break; % 如果读取到空数组，表示到达文件末尾，退出循环
    end

%更新    
    cbn_k      = PostureUpdate(cbn_k_1,v_k_1n,position_k_1,data_k_1,data_k);
    %yes代表进行零速修正，no代表不进行零速修正
    v_kn       = Speedupdate(cbn_k_1,v_k_2n,v_k_1n,position_k_2,position_k_1,data_k_1,data_k,"no");
    position_k = Locationupdate(v_k_1n,v_kn,position_k_1);



%结果格式化输出，需要什么内容，手动调整函数参数，将会输出到文件中！！！！！！！！！！
    timek = data_k(1);
    output(fresult2,cbn_k,v_kn,position_k,timek);
%计算结果与参考结果对比
%如果参考数据结束，则停止对比和更新
    dataTrue=TimeAalignment(f4Nav,10,timek,"4nav");
    if isempty(dataTrue)
        break; 
    end
    NE_compute(position_k,fNE2);
    contrast(fcontrast2,dataTrue,cbn_k,v_kn,position_k,timek);

%一个时刻的姿态、位置、速度更新后，它们就变为"上一时刻"的信息
   data_k_1     = data_k;
   cbn_k_1      = cbn_k;
   v_k_2n       = v_k_1n;
   v_k_1n       = v_kn;
   position_k_2 = position_k_1;
   position_k_1 = position_k;
  
end
%%
%画图分析
DataPlot("NE02.txt");%,"contrast2.txt","4plot.txt"
%%
% 关闭文件
fclose(fIMU);
fclose(fPureIns);
fclose(fcontrast0);
fclose(fcontrast1);
fclose(fcar);
fclose(fresult0);
fclose(fresult1);
fclose(fresult2);
fclose(fcontrast2);
fclose(f4Nav);
fclose(fNE1);
fclose(fNE2);
fclose(fNETrue);